/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-12-09     MBW       the first version
 */
#include "sys.h"
#include "maths.h"
#include "filter.h"
#include "string.h"
#include "stdlib.h"
#include "stdio.h"
#include "drv_common.h"
#include "uart_config.h"
#include "board.h"
#include <rtdevice.h>
#include "PPM.h"
#include "data_define.h"
#include "motor.h"
#define SAMPLE_UART_NAME       "uart6"      /* 串口设备名称 */

//´®¿Ú3ÖÐ¶Ï·þÎñ³ÌÐò
u8 Res=0,d1=0,i=0;
u8 sbus_sta=0;
u8 USART_FLAG=0;
u8 len;
u8 USART3_DATA[25];
u8 USART_RX_BUF[25];     //½ÓÊÕ»º³å,×î´óUSART_REC_LEN¸ö×Ö½Ú.
u16 USART_RX_STA=0;       //½ÓÊÕ×´Ì¬±ê¼Ç
#define PWM 500
#define lun 3
#define x1 1
#define y1 1
#define x2 0.7
#define y2 1
#define x3 0.7
#define y3 1
#define x4 1
#define y4 1
#define zhongzhi1 1000

#define AIN1   PAout(4)
#define AIN2   PAout(5)
#define BIN1   PAout(2)
#define BIN2   PAout(1)


u16 channel_1   = 0;
u16 channel_2   = 0;
u16 channel_3   = 0;
u16 channel_4   = 0;
u16 channel_5   = 0;
u16 channel_6   = 0;
u16 channel_7   = 0;
u16 channel_8   = 0;
u16 channel_9   = 0;
u16 channel_10  = 0;
u16 channel_11  = 0;
u16 channel_12  = 0;
/* 用于接收消息的信号量 */
static rt_device_t serial;
static struct serial_configure config2 = RT_SERIAL_CONFIG_DEFAULT;  /* 初始化配置参数 */
static uint8_t _datatemp[50];
static float check_time_ms[3];
void _Channels(void);
//½âÎöÍ¨µÀÖµ
u16 _channels[16];
void sbus_task(void *p)
{
   u8 i;
   u32 t_now,t_preiod,t_pre ;
   void *delay;
   delay = delay_until_create("delay");  // 创建相对延时
    while(1)
    {
        if(sbus_sta == 1)
        {
            _Channels();               //½âÎöÊý¾Ý²¢×ª»»
            channel_1 = _channels[0];
            channel_2 = _channels[1];
            channel_3 = _channels[2];
            channel_4 = _channels[3];
            channel_5 = _channels[4];
            channel_6 = _channels[5];
            channel_7 = _channels[6];
            channel_8 = _channels[7];
            channel_9 = _channels[8];
            channel_10 = _channels[9];
            channel_11 = _channels[10];
            channel_12 = _channels[11];
//            PPM_con.PPM_thrust = channel_3*1.25+228.75;//控制舵机时的占空比转化
            channel_1 = constrainf(channel_1,201,1801);
            channel_2 = constrainf(channel_2,201,1801);
            channel_3 = constrainf(channel_3,201,1801);
            channel_4 = constrainf(channel_4,201,1801);
            PPM_con.PPM_pitch  = channel_2-1001;
            PPM_con.PPM_roll   = channel_4-1001;
            PPM_con.PPM_yaw    = 0.225*channel_1-225.225;

            control_data.set_position.x = PPM_con.PPM_pitch;
            control_data.set_position.y = PPM_con.PPM_roll;
            control_data.set_angel.z    = PPM_con.PPM_yaw;

            PPM_con.PPM_thrust = 0.625*channel_3-125.625;
            if(PPM_con.PPM_thrust>20)control_data.motor_power = 1;
            else  control_data.motor_power = 0;
//            printf("chan1:%d chan2:%d chan3:%d chan4:%d chan5:%d chan6:%d \r\n"
//                    ,channel_1,channel_2,channel_3,channel_4,channel_5,channel_6);

//            TIM4->CCR1 = (u16)PPM_con.PPM_thrust+1000;//设置第一通道PC6
//            TIM4->CCR2 = channel_3;//设置第二通道PC7
//            TIM4->CCR3 = channel_3; //设置第三通道PC8
//            TIM4->CCR4 = channel_3; //设置第四通道PC69

        }
        else {
//          printf("...error ! chan1:%d chan2:%d chan3:%d chan4:%d chan5:%d chan6:%d\r\n"
//                ,channel_1,channel_2,channel_3,channel_4,channel_5,channel_6);
        }
//        printf("--%x %x %x %x\r\n" ,USART_RX_BUF[21],USART_RX_BUF[22],USART_RX_BUF[23],USART_RX_BUF[24]);
//      printf("...b24 ! %x\r\n", USART_RX_BUF[23]);
        //**********任务运行时间统计**********
        t_now = rt_tick_get();//要赋值给整型数据
        t_preiod=t_now-t_pre;
        task_time1.sbus_time = 1000/t_preiod;
        t_pre = t_now;
  //        rt_thread_mdelay(5);
        delay_until_run(delay, 10);
    }
}
void _Channels(void)
{
    _channels[0]  = ((USART3_DATA[1]|USART3_DATA[2]<<8)           & 0x07FF);
    _channels[1]  = ((USART3_DATA[2]>>3 |USART3_DATA[3]<<5)                 & 0x07FF);
    _channels[2]  = ((USART3_DATA[3]>>6 |USART3_DATA[4]<<2 |USART3_DATA[5]<<10)  & 0x07FF);
    _channels[3]  = ((USART3_DATA[5]>>1 |USART3_DATA[6]<<7)                 & 0x07FF);
    _channels[4]  = ((USART3_DATA[6]>>4 |USART3_DATA[7]<<4)                 & 0x07FF);
    _channels[5]  = ((USART3_DATA[7]>>7 |USART3_DATA[8]<<1 |USART3_DATA[9]<<9)   & 0x07FF);
    _channels[6]  = ((USART3_DATA[9]>>2 |USART3_DATA[10]<<6)                & 0x07FF);
    _channels[7]  = ((USART3_DATA[10]>>5|USART3_DATA[11]<<3)                & 0x07FF);
    _channels[8]  = ((USART3_DATA[12]   |USART3_DATA[13]<<8)                & 0x07FF);
    _channels[9]  = ((USART3_DATA[13]>>3|USART3_DATA[14]<<5)                & 0x07FF);
    _channels[10] = ((USART3_DATA[14]>>6|USART3_DATA[15]<<2|USART3_DATA[16]<<10) & 0x07FF);
    _channels[11] = ((USART3_DATA[16]>>1|USART3_DATA[17]<<7)                & 0x07FF);
    _channels[12] = ((USART3_DATA[17]>>4|USART3_DATA[18]<<4)                & 0x07FF);
    _channels[13] = ((USART3_DATA[18]>>7|USART3_DATA[19]<<1|USART3_DATA[20]<<9)  & 0x07FF);
    _channels[14] = ((USART3_DATA[20]>>2|USART3_DATA[21]<<6)                & 0x07FF);
    _channels[15] = ((USART3_DATA[21]>>5|USART3_DATA[22]<<3)                & 0x07FF);

}

/* 接收数据回调函数 */
static rt_err_t uart_input2(rt_device_t dev, rt_size_t size)
{
    char ucData;
    rt_device_read(serial, -1, &ucData, 1);
    Res=ucData;
    if((Res==0x0f) || (USART_RX_STA==1))
    {
        USART_RX_BUF[d1++]=Res;
        USART_RX_STA=1;
        if(d1==25)
        {
            USART_RX_STA=0;
            if((Res == 0x04) || (Res == 0x14) || (Res == 0x24) || (Res == 0x34) || (Res == 0x00))
            {

                for(i=0;i<25;i++)
                {
                    USART3_DATA[i] = USART_RX_BUF[i];
                }
                sbus_sta = 1;
            }
            else
            {
                for(i=0;i<50;i++)
                {
                    USART_RX_BUF[i] = 0;
                }
                sbus_sta = 0;
            }
            d1=0;
        }
    }

}
void sbus_task_init(void)
{
    rt_thread_t tid1 = RT_NULL , tid2 = RT_NULL;

    /* 查找串口设备 */
    serial = rt_device_find(SAMPLE_UART_NAME);
    if (!serial)
    {
        rt_kprintf("find %s failed!\n", SAMPLE_UART_NAME);
    }
    else rt_kprintf("find %s is ok!\n", SAMPLE_UART_NAME);
    /* step2：修改串口配置参数 */
//    config2.baud_rate = BAUD_RATE_19200;        //修改波特率为
    config2.baud_rate = 100000;        //修改波特率为
    config2.data_bits = DATA_BITS_8;           //数据位 8
    config2.stop_bits = STOP_BITS_2;           //停止位 1
    config2.bufsz     = 25;                   //修改缓冲区 buff size 为 128
    config2.parity    = 2;           //无奇偶校验位
    /* step3：控制串口设备。通过控制接口传入命令控制字，与控制参数 */
    rt_device_control(serial, RT_DEVICE_CTRL_CONFIG, &config2);
    /* step4：打开串口设备。以中断接收及轮询发送模式打开串口设备 */
    rt_device_open(serial, RT_DEVICE_FLAG_INT_RX);
    /* 以读写及中断接收方式打开串口设备 */
    rt_device_open(serial, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX);
    /* 设置接收回调函数 */
    rt_device_set_rx_indicate(serial, uart_input2);
   /* 发送字符串 */
//    rt_device_write(serial, 0, str, (sizeof(str) - 1));
    rt_thread_mdelay(10);
    rt_kprintf("flow_init %s is ok! \n",SAMPLE_UART_NAME);

    tid1 = rt_thread_create("sbus_task",sbus_task, RT_NULL, 2000, sbus_task_prio, 20);
    if (tid1 != RT_NULL)rt_thread_startup(tid1),rt_kprintf("...sbus_task is ok!\n");
}
INIT_APP_EXPORT(sbus_task_init);



int go(int vx,int vy,int wv)
{
      //×ªËÙ¼ÆËã±äÁ¿   ÐèÒª³ËÉÏÒ»¶¨±ÈÀýÒò×Ó Ó³Éäµ½pwm 0 - 1600 µÄ·¶Î§ ²Å×îÖÕ³ÉÎªµç×Ópwm
      float r1,r2,r3,r4;

      // µç»úpwm µÄ×îÖÕÖµ     Îª×ªËÙ¼ÆËã±äÁ¿µÄÕûÊý²¿·Ö¡£
        int pwm1,pwm2,pwm3,pwm4;

      // ¼ÆËãvx vy wv ¾àÀë Ò¡¸ËÖÐÐÄµÄ¾àÀë ½«Õâ¸ö¿´ÊÓ×ö ¸ø¶¨µÄËÙ¶È
        vx = vx - zhongzhi1;
    vy = -(vy - zhongzhi1);
    wv = -(wv - zhongzhi1);

      // ÉèÖÃÎ¢Ð¡Ò¡¸ËÈÅ¶¯ ¶¼ÊÇÎªÁã       ¼õÉÙÎó²Ù×÷
        if(vx<40 && vx > -40)
        { vx = 0;}
        if(vy<40 && vy > -40)
        { vy = 0;}
        if(wv<130 && wv > -130)  // Ë®Æ½×óÓÒÍ¨µÀÊýÖµÉèÖÃ  ¼õÉÙÎó²Ù×÷Á¿
        { wv = 0;}

        r1 = (vx*1.3-vy*1.3+wv*-(x1+y1));
        r2 = (vx*1.3+vy*1.3+wv*-(x2+y2));
        r3 = (vx*1.3-vy*1.3+wv*(x3+y3));
        r4 = (vx*1.3+vy*1.3+wv*(x4+y4));

        pwm1 = (int)(r1);
        pwm2 = (int)(r2);
        pwm3 = (int)(r3);
        pwm4 = (int)(r4);
        if(abs(pwm1) >= 1600) pwm1 = 1600;
        if(abs(pwm2) >= 1600) pwm2 = 1600;
        if(abs(pwm3) >= 1600) pwm3 = 1600;
        if(abs(pwm4) >= 1600) pwm4 = 1600;

        if (pwm1 > 0){AIN1 = 0;AIN2 = 1;BIN1 = 0;BIN2 = 1;TIM_SetTIM3Compare1(pwm1);}
    else if(pwm1 == 0){AIN1 = 0;AIN2 = 0;BIN1 = 0;BIN2 = 1;TIM_SetTIM3Compare1(pwm1);}
    else if(pwm1 < 0){AIN1 = 1;AIN2 = 0;BIN1 = 0;BIN2 = 1;pwm1 = -pwm1;TIM_SetTIM3Compare1(pwm1);}


        if (pwm2 > 0){AIN1 = 1;AIN2 = 0;BIN1 = 0;BIN2 = 1;TIM_SetTIM3Compare2(pwm2);}
        else if(pwm2 == 0){AIN1 = 0;AIN2 = 0;BIN1 = 0;BIN2 = 1;TIM_SetTIM3Compare2(pwm2);}
    else if(pwm2 < 0){AIN1 = 0;AIN2 = 1;BIN1 = 0;BIN2 = 1;pwm2 = -pwm2;TIM_SetTIM3Compare2(pwm2);}

        printf("\r\npwm1=%d\r\n",pwm1);
        printf("\r\npwm2=%d\r\n",pwm2);
        printf("\r\npwm3=%d\r\n",pwm3);
        printf("\r\npwm4=%d\r\n",pwm4);
        printf("\r\n \r\n");
        printf("\r\n \r\n");
        delay_ms(1000);
        delay_ms(1000);
        return 0;
}
